#pragma config(Hubs,  S1, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S2,     irsSensor,           sensorLowSpeed)
#pragma config(Sensor, S3,     compassSensor,       sensorI2CCustom)
#pragma config(Sensor, S4,     accelSensor,         sensorLowSpeed)
#pragma config(Motor,  motorA,          A,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorB,          B,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          C,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     A1,            tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     A2,            tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     A3,            tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_2,     A4,            tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C1_1,    S1,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_2,    S2,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_3,    S3,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_4,    S4,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_5,    S5,                   tServoNormal)
#pragma config(Servo,  srvo_S1_C1_6,    S6,                   tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "drivers/HTIRS2-driver.h"
#include "drivers/HTMC-driver.h"
#include "drivers/HTAC-driver.h"

int startingHeading;
int dirFR; //Direction forward reverse

void initializeRobot()
{
  startingHeading = HTMCsetTarget(compassSensor);
  dirFR = 1;

  servoChangeRate[S1] = 2;
  servoChangeRate[S2] = 2;
  servoChangeRate[S3] = 2;
  servoChangeRate[S4] = 2;
  wait1Msec(500);
  servo[S2] = 220;
  servo[S4] = 30;
  wait1Msec(500);
	servo[S1] = 190;
  servo[S3] = 70;
}

void move( int distance ) //Distance in encoder values always positive, direction forward reverse (F: 1 tile = 2500, R: 1 tile = 3000)
{
  //nMotorEncoder[A1] = 0;
  nMotorEncoder[A2] = 0;
  //nMotorEncoderTarget[A1] = distance; //calibrate this
  if( dirFR == 1 )
  {

  } else if( dirFR == -1 ) {
    distance = distance * 6/5;
  } else { //safety net
    dirFR = 1;
  }

  nMotorEncoderTarget[A2] = distance;

  motor[A1] = 100 * dirFR;
  motor[A2] = 50 * dirFR;

  while( abs(nMotorEncoder[A2]) < distance )
  {

  }

  motor[A1] = 0;
  motor[A2] = 0;
}

void turn( int degree ) //Positive is a right turn
{
  HTMCsetTarget(compassSensor);

  if( degree > 0 )
  {
    dirFR = -dirFR;
    degree = -degree;
  }

  motor[A1] = -50;
  motor[A2] = 100;

  while( HTMCreadRelativeHeading(compassSensor) > degree )
  {

  }

  motor[A1] = 0;
  motor[A2] = 0;

  HTMCsetTarget(compassSensor, startingHeading);
}

void irsSeek( int targetZoneHeading )
{
  HTMCsetTarget(compassSensor, targetZoneHeading);
  turn( -HTMCreadRelativeHeading(compassSensor) );

  int irsSensorValue = HTIRS2readDCDir(irsSensor);
  int irsSensorSign;

  while( irsSensorValue != 5 )
  {
    if(HTMCreadRelativeHeading(compassSensor) > -60 && HTMCreadRelativeHeading(compassSensor) < 60)
    {
      irsSensorValue = HTIRS2readDCDir(irsSensor);
      irsSensorSign = sgn(irsSensorValue - 5);
      motor[A1] = irsSensorSign * 50;
      motor[A2] = -irsSensorSign * 50;
    } else { //failed to detect
      turn( -HTMCreadRelativeHeading(compassSensor) );
      HTMCsetTarget(compassSensor, startingHeading);
      return;
    }
  }

  motor[A1] = 0;
  motor[A2] = 0;

  HTMCsetTarget(compassSensor, startingHeading);

}


//----------------------------------Different Autonomous--------------------

void strategy_EmptyBack() //5 points
{
  motor[A1] = 50;
  motor[A2] = 50;
}

void strategy_EmptyFront() //10 points
// int posRB, int posIO ) //position red/blue, position inner/outer
{
  //make different positions later

  //Get off Ramp

  turn(-90);
  move(5000);
  turn(-45);
  move(7072);


  /*
  frontZoneHeading = -135;
  backZoneHeading = 45

  while(true) //loop
  {
    irsSeek( frontZoneHeading ); //turn to the front zone
    motor[A1] = 50; //go forward
    motor[A2] = 50;
    wait1Msec(3); //repeat after 3 seconds
  }
  */

}

void strategy_BowledFront() //20 points
{
  //Get OFf Ramp

  turn(-90);
  move(5000);
  turn(-45);
  move(8000);

}

void strategy_BowledBack() //25 points
{
  //Get OFf Ramp

  turn(-90);
  move(5000);
  turn(-45);
  move(2000);
  servo[S2] = 160;
  turn(-200);
  move(11000);

}

void strategy_BowledBackParkFront() //30 points
{
  //Get OFf Ramp

  turn(-90);
  move(5000);
  turn(-45);
  move(2000);
  servo[S2] = 160;
  turn(-200);
  move(11000);
  servo[S2] = 220;
  dirFR = -1;
  move(3536);
  turn(45-startingHeading);
  move(18000);

}


//--------------------------Get off Ramp-----------------

void ramp_Accel()
{
  move(2500);

  int xpos, ypos, zpos;
  HTACreadAllAxes(accelSensor, xpos, ypos, zpos);
  xpos = xpos + 31;
  ypos = ypos - 13;
  zpos = zpos - 206;

  while( zpos < 0 )
  {
    nMotorEncoder[A2] = 0;

    nMotorEncoderTarget[A2] = 500;

    motor[A1] = 60;
    motor[A2] = 30;

    while( abs(nMotorEncoder[A2]) < 500 )
    {

    }

    motor[A1] = 0;
    motor[A2] = 0;
    wait1Msec(2000);
    HTACreadAllAxes(accelSensor, xpos, ypos, zpos);
    zpos = zpos - 206;
    }
}

/*
void ramp_Color()
{
  move(500);

  while( HTCS2readColor(accelSensor) == 0 )
  {
    move(100);
  }

  while( HTCS2readColor(accelSensor) != 0 )
  {
    nMotorEncoderTarget[A1] = 100;
    nMotorEncoderTarget[A2] = 100;
    nMotorEncoder[A1] = 0;
    nMotorEncoder[A2] = 0;

    motor[A1] = 30;
    motor[A2] = 30;

    wait1Msec(500);
  }
}
*/


task main()
{
  initializeRobot();

  wait1Msec(2000);

  //waitForStart();

  int frontZoneHeading;
  int backZoneHeading;

  ramp_Accel()

}
